﻿#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QDebug>
#include <QVector3D>

typedef struct {
    double m[3][3];
} Matrix3x3;
typedef struct {
    double w, x, y, z;
} Quaternion;

void quaternionToRotationMatrix(double w, double x, double y, double z, Matrix3x3 *R)
{
    R->m[0][0] = 1.0 - 2.0 * y * y - 2.0 * z * z;
    R->m[0][1] = 2.0 * x * y - 2.0 * w * z;
    R->m[0][2] = 2.0 * x * z + 2.0 * w * y;

    R->m[1][0] = 2.0 * x * y + 2.0 * w * z;
    R->m[1][1] = 1.0 - 2.0 * x * x - 2.0 * z * z;
    R->m[1][2] = 2.0 * y * z - 2.0 * w * x;

    R->m[2][0] = 2.0 * x * z - 2.0 * w * y;
    R->m[2][1] = 2.0 * y * z + 2.0 * w * x;
    R->m[2][2] = 1.0 - 2.0 * x * x - 2.0 * y * y;
}

Quaternion matrixToQuaternion(double m[3][3]) {
    Quaternion q;
    double trace = m[0][0] + m[1][1] + m[2][2];
    double s;

    if (trace > 0) {
        s = sqrt(trace + 1.0);
        q.w = s * 0.5;
        s = 0.5 / s;
        q.x = (m[2][1] - m[1][2]) * s;
        q.y = (m[0][2] - m[2][0]) * s;
        q.z = (m[1][0] - m[0][1]) * s;
    } else {
        // 查找最大的对角线元素以确定四元数的实部
        int i = 0;
        if (m[1][1] > m[0][0]) i = 1;
        if (m[2][2] > m[i][i]) i = 2;
        int j = (i + 1) % 3;
        int k = (j + 1) % 3;

        s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
        switch (i) {
        case 0:
            q.w = (m[2][1] - m[1][2]) * 0.5 / s;
            q.x = s * 0.5;
            q.y = (m[0][1] + m[1][0]) * 0.5 / s;
            q.z = (m[0][2] + m[2][0]) * 0.5 / s;
            break;
        case 1:
            q.w = (m[0][2] - m[2][0]) * 0.5 / s;
            q.x = (m[0][1] + m[1][0]) * 0.5 / s;
            q.y = s * 0.5;
            q.z = (m[1][2] + m[2][1]) * 0.5 / s;
            break;
        case 2:
            q.w = (m[1][0] - m[0][1]) * 0.5 / s;
            q.x = (m[0][2] + m[2][0]) * 0.5 / s;
            q.y = (m[1][2] + m[2][1]) * 0.5 / s;
            q.z = s * 0.5;
            break;
        }
    }

    // 可选：四元数正规化
    double norm = sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    q.w /= norm;
    q.x /= norm;
    q.y /= norm;
    q.z /= norm;
    return q;
}
QVector3D rotationMatrixToEulerAngles(double R[3][3])
{
    double sy = sqrt(R[0][0] * R[0][0] +  R[1][0] * R[1][0]);
    bool singular = sy < 1e-6;
    double x,y,z;
    if (!singular)
    {
        x = atan2(R[2][1] , R[2][2]);
        y =atan2(-R[2][0], sy);
        z = atan2(R[1][0], R[0][0]);
    }
    else
    {
        x = atan2(-R[1][2], R[1][1]);
        y = atan2(-R[2][0], sy);
        z = 0;
    }
    return QVector3D(x, y, z);
}
void eulertomatrix(double a,double b,double c,Matrix3x3* R)
{
    double cosyaw= cos(c);
    double sinyaw= sin(c);
    double cospitch= cos(b);
    double sinpitch= sin(b);
    double cosroll= cos(a);
    double sinroll= sin(a);
    R->m[0][0] = cosyaw * cospitch;
    R->m[0][1] = cosyaw * sinpitch * sinroll - sinyaw * cosroll;
    R->m[0][2] = cosyaw * sinpitch * cosroll + sinyaw * sinroll;
    R->m[1][0] = sinyaw * cospitch;
    R->m[1][1] = sinyaw * sinpitch * sinroll + cosyaw * cosroll;
    R->m[1][2] = sinyaw * sinpitch * cosroll - cosyaw * sinroll;
    R->m[2][0] = -sinpitch;
    R->m[2][1] = cospitch * sinroll;
    R->m[2][2] = cospitch * cosroll;
}

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    connect(ui->fkpushButton,SIGNAL(clicked(bool)),this,SLOT(fk()));
    connect(ui->ikpushButton,SIGNAL(clicked(bool)),this,SLOT(ik()));
    ui->q1lineEdit->setText(QString::number(-0.791));
    ui->q2lineEdit->setText(QString::number(-4.463));
    ui->q3lineEdit->setText(QString::number(77.481));
    ui->q4lineEdit->setText(QString::number(10.547));
    ui->q5lineEdit->setText(QString::number(-95.795));
    ui->q6lineEdit->setText(QString::number(-5.886));
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::fk()
{
    double q1 = ui->q1lineEdit->text().toDouble();
    double q2 = ui->q2lineEdit->text().toDouble();
    double q3 = ui->q3lineEdit->text().toDouble();
    double q4 = ui->q4lineEdit->text().toDouble();
    double q5 = ui->q5lineEdit->text().toDouble();
    double q6 = ui->q6lineEdit->text().toDouble();
    //double offset[6] = { -90,90,0,90,0,-90 };
    QVector<double> Q;
//    Q.push_back((q1+offset[0])*DEG2RAD);Q.push_back((q2+offset[1])*DEG2RAD);Q.push_back((q3+offset[2])*DEG2RAD);
//    Q.push_back((q4+offset[3])*DEG2RAD);Q.push_back((q5+offset[4])*DEG2RAD);Q.push_back((q6+offset[5])*DEG2RAD);
    Q<<q1<<q2<<q3<<q4<<q5<<q6;
    T = CIRT_LBR::LBR_fk(Q);

    qDebug()<<T.px<<T.py<<T.pz;
    ui->nxlineEdit->setText(QString::number(T.nx));ui->nylineEdit->setText(QString::number(T.ny));ui->nzlineEdit->setText(QString::number(T.nz));
    ui->oxlineEdit->setText(QString::number(T.ox));ui->oylineEdit->setText(QString::number(T.oy));ui->ozlineEdit->setText(QString::number(T.oz));
    ui->axlineEdit->setText(QString::number(T.ax));ui->aylineEdit->setText(QString::number(T.ay));ui->azlineEdit->setText(QString::number(T.az));
    ui->pxlineEdit->setText(QString::number(T.px));ui->pylineEdit->setText(QString::number(T.py));ui->pzlineEdit->setText(QString::number(T.pz));
    ui->ppxlineEdit->setText(QString::number(T.px*1000));
    ui->ppylineEdit->setText(QString::number(T.py*1000));
    ui->ppzlineEdit->setText(QString::number(T.pz*1000));
    T.toeulerangle();
    //ui->prxlineEdit->setText();
    //ui->prylineEdit->setText();
    //ui->przlineEdit->setText();
    double M[3][3];
    M[0][0]=T.nx;M[0][1]=T.ox;M[0][2]=T.ax;
    M[1][0]=T.ny;M[1][1]=T.oy;M[1][2]=T.ay;
    M[2][0]=T.nz;M[2][1]=T.oz;M[2][2]=T.az;
    Quaternion q=matrixToQuaternion(M);
    qDebug()<<" w x y z :"<<q.w<<q.x<<q.y<<q.z;
    ui->lineEdit_qw->setText(QString::number(q.w));
    ui->lineEdit_qx->setText(QString::number(q.x));
    ui->lineEdit_qy->setText(QString::number(q.y));
    ui->lineEdit_qz->setText(QString::number(q.z));
    QVector3D eul = rotationMatrixToEulerAngles(M);
    ui->prxlineEdit->setText(QString::number(eul.x()*180/M_PI));
    ui->prylineEdit->setText(QString::number(eul.y()*180/M_PI));
    ui->przlineEdit->setText(QString::number(eul.z()*180/M_PI));
}

void MainWindow::ik()
{
//    double nx = ui->nxlineEdit->text().toDouble();double ny = ui->nylineEdit->text().toDouble();double nz = ui->nzlineEdit->text().toDouble();
//    double ox = ui->oxlineEdit->text().toDouble();double oy = ui->oylineEdit->text().toDouble();double oz = ui->ozlineEdit->text().toDouble();
//    double ax = ui->axlineEdit->text().toDouble();double ay = ui->aylineEdit->text().toDouble();double az = ui->azlineEdit->text().toDouble();
//    double px = ui->pxlineEdit->text().toDouble();double py = ui->pylineEdit->text().toDouble();double pz = ui->pzlineEdit->text().toDouble();
//    CIRT_LBR::Transform T(nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz);
    QVector<QVector<double> > Q_set = CIRT_LBR::LBR_ik(T);
    double *q,*qq;
    double Qs[6] = { 0, 0, 0, 0, 0, 0 };
    qq = &Qs[0];
    double q_sols[48];
    QVector<QVector<double> > num_sols;
    //num_sols=CIRT_LBR::LBR_inverse(T,q_sols,qq);
    for(int i=0;i<num_sols.size();++i)
    {
        qDebug()<<"lbr_inverse slutions "<<i<<" : "<<num_sols.at(i);
        //q_sols[i*6+0]<<q_sols[i*6+1]<<q_sols[i*6+2]<<q_sols[i*6+3]<<q_sols[i*6+4]<<q_sols[i*6+5];
    }
    qDebug()<<"lbr_inverse "<<Qs[0]<<Qs[1]<<Qs[2]<<Qs[3]<<Qs[4]<<Qs[5];

    ui->inverseqlistWidget->clear();
    if(!Q_set.size())
    {
        ui->inverseqlistWidget->addItem("no solution!");
    }
    else
    {
        for(unsigned int i=0;i<Q_set.size();++i)
        {

            CIRT_LBR::Transform t = CIRT_LBR::LBR_fk(Q_set[i]);
            t -= T;
            double error = t.abs();
            ui->inverseqlistWidget->addItem(q2str(Q_set[i]));
            ui->inverseqlistWidget->addItem(QString(" error: ").append(QString::number(error)));
        }
    }
    QVector<double> current;
    current<<ui->q1lineEdit->text().toFloat()<<ui->q2lineEdit->text().toFloat()<<ui->q3lineEdit->text().toFloat()
            <<ui->q4lineEdit->text().toFloat()<<ui->q5lineEdit->text().toFloat()<<ui->q6lineEdit->text().toFloat();
    QVector<float> selectsult=CIRT_LBR::nearstsloution(Q_set,current);
    qDebug()<<"ik result is "<<selectsult;

}

QString MainWindow::q2str(const QVector<double> &Q)
{
    QString str;
    str.append("Q [ ");
    for(unsigned int i=0;i<Q.size();++i)
    {
        if(std::abs(Q[i])<1e-6)
        {
        str.append("0").append(" ");
        }
        else
        {
            str.append(QString::number(Q[i])).append(" ");
        }
    }
    str.append("]");
    return str;
}

void MainWindow::on_fkpushButton_clicked()
{

}


void MainWindow::on_pushButton_to_euler_clicked()
{
    double w=ui->lineEdit_qw->text().toDouble();
    double x=ui->lineEdit_qx->text().toDouble();
    double y=ui->lineEdit_qy->text().toDouble();
    double z=ui->lineEdit_qz->text().toDouble();
    Matrix3x3 R;
    quaternionToRotationMatrix(w,x,y,z,&R);

    QVector3D el=rotationMatrixToEulerAngles(R.m);
    qDebug()<<"el =="<<el;
    ui->prxlineEdit->setText(QString::number(el.x()*180/M_PI));
    ui->prylineEdit->setText(QString::number(el.y()*180/M_PI));
    ui->przlineEdit->setText(QString::number(el.z()*180/M_PI));
}


void MainWindow::on_pushButton_to_wxyz_clicked()
{
    double a=ui->prxlineEdit->text().toDouble()*M_PI/180;
    double b=ui->prylineEdit->text().toDouble()*M_PI/180;
    double c=ui->przlineEdit->text().toDouble()*M_PI/180;
    Matrix3x3 R;
    eulertomatrix(a,b,c,&R);
    qDebug()<<"stand matrix";
    qDebug()<<R.m[0][0]<<R.m[0][1]<<R.m[0][2];
    qDebug()<<R.m[1][0]<<R.m[1][1]<<R.m[1][2];
    qDebug()<<R.m[2][0]<<R.m[2][1]<<R.m[2][2];
    QVector<double> pos;
    pos<<ui->ppxlineEdit->text().toDouble()<<ui->ppylineEdit->text().toDouble()<<ui->ppzlineEdit->text().toDouble()<<
        ui->prxlineEdit->text().toDouble()<<ui->prylineEdit->text().toDouble()<<ui->przlineEdit->text().toDouble();
    qDebug()<<"pos == "<<pos;
    CIRT_LBR::Transform T(pos);
    qDebug()<<"T matrix";
    qDebug()<<T.nx<<T.ox<<T.ax;
    qDebug()<<T.ny<<T.oy<<T.ay;
    qDebug()<<T.nz<<T.oz<<T.az;

    Quaternion q = matrixToQuaternion(R.m);
    qDebug()<<q.w<<q.x<<q.y<<q.z;
    ui->lineEdit_qw->setText(QString::number(q.w));
    ui->lineEdit_qx->setText(QString::number(q.x));
    ui->lineEdit_qy->setText(QString::number(q.y));
    ui->lineEdit_qz->setText(QString::number(q.z));
}

